#include "common.h"

#ifdef USE_PCA

#include "pca.h"

typedef enum edgeType_e {
	ET_RISING=0,
	ET_FALLING,
} eEdgeType;

SBIT(PIN_STEERING,0xB0,4);//sbit PIN_STEERING=P3^4;

//Power transistor controller signal
//|-----+---VCC
//Q1     Q3
//|--M--|
//Q2     Q4
//|-----+---GND
SBIT(PIN_Q1,0xA0,1);//sbit PIN_Q1=P2^1;
SBIT(PIN_Q2,0xA0,0);//sbit PIN_Q2=P2^0;
SBIT(PIN_Q3,0x90,6);//sbit PIN_Q3=P1^6;
SBIT(PIN_Q4,0x90,4);//sbit PIN_Q4=P1^4;
#define MOTOR_FORWARD()		PIN_Q3=0;PIN_Q2=0;PIN_Q4=1
#define MOTOR_BACKWARD()	PIN_Q1=0;PIN_Q4=0;PIN_Q2=1
#define MOTOR_GEAR(x) \
	if( !PIN_Q2 ) { \
		PIN_Q1=x; \
	} \
	else if( !PIN_Q4 ) { \
		PIN_Q3=x; \
	}
#define MOTOR_BRAKE()			PIN_Q1=0;PIN_Q3=0;PIN_Q2=1;PIN_Q4=1

#define PCA_STEERING 0	//use PCA0
#define PCA_GEAR 1//use PCA1

extern char vehicleSteering;
extern char vehicleGear;
static volatile bool isBrake=false;

void PCA_INIT()
{
	CMOD=0;// pca clock: Fosc/12, disable CF(PCA) interrupt
	CCON=0;
	CL=CH=0x00;
	EPCA_LVD=1;//enable IE bit
	PPCA_LVD=1;//priority high

	CCAPM(PCA_STEERING)=0x49; //clock mode	//enable PCA##PCA_STEERING interrupt
	CCAPL(PCA_STEERING)=0xff;	//first 32.768ms
	CCAPH(PCA_STEERING)=0xff;
	CCAPM(PCA_GEAR)=0x49; //clock mode	//enable PCA##PCA_GEAR interrupt
	CCAPL(PCA_GEAR)=0xff;	//first 32.768ms
	CCAPH(PCA_GEAR)=0xff;
	P3M1|=0x10;//PIN_STEERING output current high
	P2M1|=0x03;
	P1M1|=0x50;
}

//////////////////////////////////////////////////////////////////////////
//	PCA INTERRUPT
//////////////////////////////////////////////////////////////////////////
void PCAINT(void) interrupt PCALVD_VECTOR
{
	static eEdgeType steeringEdgeType=ET_RISING;
	static char lastSteering=0;
	static eEdgeType gearEdgeType=ET_RISING;
	static uchar lastGear=0;
	unsigned char lastGearADjust=0;
	unsigned int counter,CHL;
	unsigned char CHbak,CLbak;

	//PCA interrupt
	if(CF==1)
		CF=0;


	//PCA interrupt PCA_STEERING
	if(CCF(PCA_STEERING)==1)
	{
		CCF(PCA_STEERING)=0;

		for(;;)
		{
			CHbak=CH;
			CLbak=CL;
			if(CHbak==CH)
				break;
		}
		CHL=CHbak;CHL<<=8;CHL|=CLbak;

		if(ET_RISING==steeringEdgeType)
		{
			lastSteering=vehicleSteering;
			if(lastSteering>32)
				lastSteering=32;
			else if(lastSteering<-32)
				lastSteering=-32;

			counter=(uint)(3288+(int)(lastSteering)*10);//(0.5ms(base time)+1ms(central time point))/(1/(Fosc(24)/12))
			PIN_STEERING=1;
			steeringEdgeType=ET_FALLING;
		}
		else
		{
			counter=(uint)(36712-(int)(lastSteering)*10);//(20ms-0.5ms(base time)-1ms(central time point))/(1/(Fosc(24)/12))
			PIN_STEERING=0;
			steeringEdgeType=ET_RISING;
		}

		counter+=CHL;
		CCAPM(PCA_STEERING)&=0xBE;//disable compare, disable PCA##PCA_STEERING interrupt
		CCAPL(PCA_STEERING)=counter&0xff;
		CCAPH(PCA_STEERING)=(counter>>8)&0xff;
		CCAPM(PCA_STEERING)|=0x41;//enable compare, enable PCA##PCA_STEERING interrupt
	}

	//PCA interrupt PCA_GEAR
	if(CCF(PCA_GEAR)==1) {
		CCF(PCA_GEAR)=0;

		//brake override system ;)
		if(isBrake) {
			//this will be call per 32ms for the worst case (pca comparator rematch with no reload)
			MOTOR_BRAKE();
			gearEdgeType=ET_RISING;
		}
		else {

			//gear dir
			if(vehicleGear<0) {
				MOTOR_FORWARD();
				lastGearADjust=-vehicleGear;
			}
			else if(vehicleGear>0){
				MOTOR_BACKWARD();
				lastGearADjust=vehicleGear;
			}
			//gear
			if(lastGearADjust==0) {
				MOTOR_GEAR(0);
				gearEdgeType=ET_RISING;
			}
			else if(lastGearADjust >=32) {
				lastGearADjust=32;
				MOTOR_GEAR(1);
				gearEdgeType=ET_RISING;
			}
			else {

#define PCA_GEAR_INTERVAL_EXPAND 6 //16.384ms//feq=32<<PCA_GEAR_INTERVAL_EXPAND*0.5 (32--pwm resolution; 0.5--PCA clock)

				for(;;)
				{
					CHbak=CH;
					CLbak=CL;
					if(CHbak==CH)
						break;
				}
				CHL=CHbak;CHL<<=8;CHL|=CLbak;

				if(ET_RISING==gearEdgeType) {
					lastGear=lastGearADjust;

					counter=((uint)(lastGear))<<PCA_GEAR_INTERVAL_EXPAND;
					MOTOR_GEAR(1);
					gearEdgeType=ET_FALLING;
				}
				else {
					counter=((uint)(32-lastGear))<<PCA_GEAR_INTERVAL_EXPAND;
					MOTOR_GEAR(0);
					gearEdgeType=ET_RISING;
				}
				counter+=CHL;
				CCAPM(PCA_GEAR)&=0xBE;//disable compare, disable PCA##PCA_GEAR interrupt
				CCAPL(PCA_GEAR)=counter&0xff;
				CCAPH(PCA_GEAR)=(counter>>8)&0xff;
				CCAPM(PCA_GEAR)|=0x41;//enable compare, enable PCA##PCA_GEAR interrupt

			}
		}//end of if(isBrake)
	}//end of if(CCF(PCA_GEAR)==1)
}

void PCA_SETFREE() {
	vehicleGear=0;
//	MOTOR_BRAKE();
}

void PCA_SETBRAKE(bool b) {
	isBrake=b;
}

#endif
